#include "SensorDataInput.h"
#include "InterpolateLinear.h"

SensorDataInput::SensorDataInput (const std::string &name,
		const std::string &intrinsicFile, const std::string &extrinsicFile) :
		__depthMetaData (NULL), __rgbMetaData (NULL), _name (name),
		_p_preprocessTempDepth (NULL), _p_preprocessTempRgb (NULL)
{
	if (extrinsicFile != "")
		_readExtrinsic (extrinsicFile);
	__intrinsic.readFrom (intrinsicFile.c_str ());
	_p_depthImage = cvCreateImage (cvSize (
			(unsigned int)__intrinsic.fw (),
			(unsigned int)__intrinsic.fh ()), 16, 1);
	_p_rgbImage = cvCreateImage (cvSize (
			(unsigned int)__intrinsic.fw (),
			(unsigned int)__intrinsic.fh ()), 8, 3);
}

SensorDataInput::~SensorDataInput ()
{
	if (__depthMetaData) cvReleaseImage (&__depthMetaData);
	if (__rgbMetaData) cvReleaseImage (&__rgbMetaData);
	if (_p_preprocessTempDepth) cvReleaseImage (&_p_preprocessTempDepth);
	if (_p_preprocessTempRgb) cvReleaseImage (&_p_preprocessTempRgb);
	if (_p_depthImage) cvReleaseImage (&_p_depthImage);
	if (_p_rgbImage) cvReleaseImage (&_p_rgbImage);
}

const IplImage* SensorDataInput::getDepthImage () const
{
	return _p_depthImage;
}
IplImage* SensorDataInput::getDepthImage ()
{
	return _p_depthImage;
}

const IplImage* SensorDataInput::getRGBImage () const
{
	return _p_rgbImage;
}
IplImage* SensorDataInput::getRGBImage ()
{
	return _p_rgbImage;
}

const SensorParameter& SensorDataInput::getSensorParameter () const
{
	return __sensorParameter;
}
SensorParameter& SensorDataInput::getSensorParameter ()
{
	return __sensorParameter;
}

void SensorDataInput::showData (const std::string& titlePrefix)
{
	if (_p_depthImage) {
		cvShowImage ((titlePrefix + "Depth Image - " + _name).c_str (),
				_p_depthImage);
	}
	if (_p_rgbImage) {
		cvShowImage ((titlePrefix + "RGB Image - " + _name).c_str (),
				_p_rgbImage);
	}
}

glm::mat4x4 & SensorDataInput::getExtrinsicMat ()
{
	return __extrinsicMat;
}

const glm::mat4x4 & SensorDataInput::getExtrinsicMat () const
{
	return __extrinsicMat;
}

glm::mat4x4 & SensorDataInput::getToWorldMat ()
{
	return __inverseExtrinsicMat;
}

const glm::mat4x4 & SensorDataInput::getToWorldMat () const
{
	return __inverseExtrinsicMat;
}

std::string SensorDataInput::getName () const
{
	return _name;
}

glm::vec3 SensorDataInput::get3DPoint (float x, float y) const
{
	return (__inverseExtrinsicMat * glm::vec4 (get3DPointInCam (x, y), 1.0)).
			swizzle (glm::X, glm::Y, glm::Z);
}

glm::vec3 SensorDataInput::get3DPointInCam (float x, float y) const
{
	CvScalar depth = InterpolateLinear ().interpolate (_p_depthImage, cvPoint2D32f (x, y));
	/*float depth = CV_IMAGE_ELEM (_p_depthImage, unsigned short, (int)y, (int)x);*/
	return unprojectToCam (glm::vec3 (x, y, depth.val[0]));
}

glm::vec3 SensorDataInput::unprojectToCam (glm::vec3 pi) const
{
	return __intrinsic.unproject (pi);
}

glm::vec3 SensorDataInput::unprojectToWorld (glm::vec3 pi) const
{
	return (__inverseExtrinsicMat * glm::vec4 (unprojectToCam (pi), 1.0)).
			swizzle (glm::X, glm::Y, glm::Z);
}

void SensorDataInput::_undistortImage (IplImage *src, IplImage *dst) const
{
	const CvMat *cameraMatrix = __intrinsic.matrixCvMat ();
	const CvMat *distCoeffs = __intrinsic.distCoeffsCvMat ();
	cvUndistort2 (src, dst, cameraMatrix, distCoeffs);
}

bool SensorDataInput::getNewFrame ()
{
	if (!_grabFrame ())
		return false;
	if (__depthMetaData) {
		if (!_p_preprocessTempDepth)
			_p_preprocessTempDepth = cvCreateImage (
					cvGetSize (_p_depthImage), 16, 1);
		cvResize (__depthMetaData, _p_preprocessTempDepth);
		_undistortImage (_p_preprocessTempDepth, _p_depthImage);
	}
	if (__rgbMetaData) {
		if (!_p_preprocessTempRgb)
			_p_preprocessTempRgb = cvCreateImage (
					cvGetSize (_p_rgbImage), 8, 3);
		cvResize (__rgbMetaData, _p_preprocessTempRgb);
		_undistortImage (_p_preprocessTempRgb, _p_rgbImage);
	}
	return true;
}

SensorIntrinsic & SensorDataInput::getIntrinsic ()
{
	return __intrinsic;
}

const SensorIntrinsic & SensorDataInput::getIntrinsic () const
{
	return __intrinsic;
}

void SensorDataInput::_readExtrinsic (const std::string &file)
{
	std::ifstream fIn (file);
	for (int c = 0; c < 4; ++c)
		for (int r = 0; r < 4; ++r)
			fIn >> __extrinsicMat[c][r];
	if (!fIn)
		throw std::exception ("SensorDataInput::_readExtrinsic(): "
				"Fail to read extrinsic file");
	__inverseExtrinsicMat = glm::inverse (__extrinsicMat);
}
